/*
 * distance.c
 *
 *  Created on: May 22, 2021
 *      Author: kirk_
 */

#include "distance.h"
#include "light.h"
#include "movement.h"
#include "direction.h"

volatile uint16_t distance[3] = {2000, 2000, 2000};

void StartDistanceThread(void *argument)
{
	uint8_t times[3] = {0, 0, 0};
	int i;
	/* Init HC-SR04 */
	HCSR04_Init(&HCSR04[0], TRIG0_GPIO_Port, TRIG0_Pin, ECHO0_GPIO_Port, ECHO0_Pin);
	HCSR04_Init(&HCSR04[1], TRIG1_GPIO_Port, TRIG1_Pin, ECHO1_GPIO_Port, ECHO1_Pin);
	HCSR04_Init(&HCSR04[2], TRIG2_GPIO_Port, TRIG2_Pin, ECHO2_GPIO_Port, ECHO2_Pin);
	/* Start timer */
	osTimerStart(DistanceTimerHandle, 44);
	/* Infinite loop */
	for(;;)
	{
		osThreadSuspend(DistanceThreadHandle);
		for(i=0;i<3;i++)
		{
			if(times[i] == 0)
			{
				if(i == 0)
					Light_Turn(3, 2);
				HCSR04_Measure(&HCSR04[i]);
				distance[i] = DistanceAccumulate(distance[i], HCSR04_GetDistance(&HCSR04[i]));
			}
			else
			{
				times[i]--;
			}
		}
	}
}

void DistanceTimerCallback(void *argument)
{
  /* USER CODE BEGIN DistanceTimerCallback */
	osThreadResume(DistanceThreadHandle);
  /* USER CODE END DistanceTimerCallback */
}

uint32_t Distance_Get(uint8_t index)
{
	return distance[index>2?0:index];
}

uint16_t DistanceAccumulate(uint16_t d, uint16_t c)
{
	double dtmp1, dtmp2;
	dtmp1 = d;
	dtmp1 *= Distance_Decrease;
	dtmp2 = c;
	dtmp2 *= (1.00-Distance_Decrease);
	return (uint16_t)(dtmp1 + dtmp2 + 0.5);
}
#if 0
float DistanceAdjust(void)
{
	int i;
	uint32_t front = 0, back = 0;
	double dtmp;
	for(i=0;i<Distance_AdjustTimes;i++)
	{
		osDelay(Distance_AdjustInterval);
		front += Distance_Get(1);
		back  += Distance_Get(2);
	}
	dtmp = (double)front;
	dtmp -= (double)back;
	dtmp /= (double)i;
	dtmp /= (double)UltraSonic_Distance;
	//PRINTF("Tan is %ld.\r\n", (int32_t)(dtmp*1000));
	dtmp = atan(dtmp);
	dtmp *= (double)180;
	dtmp /= PI;
	PRINTF("Angle is %ld/1000 degrees.\r\n", (int32_t)(dtmp*1000));
	return (float)dtmp;
}
#endif

uint32_t DistanceAverage(uint8_t index, uint8_t times, uint8_t interval)
{
	uint32_t max = 0;
	uint32_t min = 0xFFFFFFFF;
	uint32_t sum = 0;
	uint32_t tmp = 0;
	uint32_t i;

	for(i=0;i<times+2;i++)
	{
		osDelay(interval);
		tmp = Distance_Get(index);
		sum += tmp;
		min = min>tmp?tmp:min;
		max = max<tmp?tmp:max;
	}

	sum -= min;
	sum -= max;

	sum += times >> 1;
	sum /= times;
	return sum;
}

void DistanceGetAverage(uint8_t times, uint8_t interval, uint32_t* d1, uint32_t* d2)
{
	uint32_t max1 = 0;
	uint32_t min1 = 0xFFFFFFFF;
	uint32_t sum1 = 0;
	uint32_t tmp1 = 0;
	uint32_t max2 = 0;
	uint32_t min2 = 0xFFFFFFFF;
	uint32_t sum2 = 0;
	uint32_t tmp2 = 0;
	uint32_t i;
	/* Get average distance */
	for(i=0;i<times+2;i++)
	{
		osDelay(interval);
		tmp1 = Distance_Get(1);
		tmp2 = Distance_Get(2);
		sum1 += tmp1;
		sum2 += tmp2;
		min1 = min1>tmp1?tmp1:min1;
		min2 = min2>tmp2?tmp2:min2;
		max1 = max1<tmp1?tmp1:max1;
		max2 = max2<tmp2?tmp2:max2;
	}
	/* Minus extreme value */
	sum1 -= min1;
	sum2 -= min2;
	sum1 -= max1;
	sum2 -= max2;
	/* Rounding */
	sum1 += Distance_AdjustTimes>>1;
	sum2 += Distance_AdjustTimes>>1;
	sum1 /= Distance_AdjustTimes;
	sum2 /= Distance_AdjustTimes;
	/* Pass data */
	*d1 = sum1;
	*d2 = sum2;
}

uint32_t DistanceAdjust(void)
{
	uint32_t d11,d12,d21,d22;
	uint8_t count = 0;
	int32_t dif1, dif2;
	int32_t d = 0;
	double dtmp;
	MoveBackward(Distance_AdjustMove);
	osDelay(2000);
	MoveForward(Distance_AdjustMove);
	osDelay(6000);
	DistanceGetAverage(Distance_AdjustTimes,Distance_AdjustInterval,&d11,&d21);
	MoveForward(Distance_AdjustMove);
	osDelay(6000);
	DistanceGetAverage(Distance_AdjustTimes,Distance_AdjustInterval,&d12,&d22);
	printf("\r\n\r\n");
	PRINTF("d11 %lu, d12 %lu.\r\n", d11,d12);
	PRINTF("d21 %lu, d22 %lu.\r\n", d21,d22);
	if(d11<1800 && d12<1800)
	{
		count++;
		dif1 = (int32_t)d11;
		dif1 -= (int32_t)d12;
		d += dif1;
		PRINTF("dif1 %ld.\r\n",dif1);
	}
	if(d21<1800 && d22<1800)
	{
		count++;
		dif2 = (int32_t)d21;
		dif2 -= (int32_t)d22;
		d += dif2;
		PRINTF("dif2 %ld.\r\n",dif2);
	}
	/* Get distance difference */
	d /= count;
	/* Calculate angle */
	dtmp = atan2((double)d, (double)Distance_AdjustMove);
	dtmp *= 180.00;
	dtmp /= PI;
	/* Turn */
	if(fabs(dtmp)<(float)Distance_AdjustTurnBound)
		Turn((float)dtmp);
	MoveBackward(Distance_AdjustMove);
	/* Debug */
	PRINTF("angle %ld.\r\n", (int32_t)(dtmp*1000));
	/* Return */
	dtmp *= (double)1000;
	if(dtmp<0)dtmp = -dtmp + (double)1000000;
	return (uint32_t)dtmp;
}

uint8_t DistanceGetAngle(double *angle)
{
	uint16_t d1, d2;
	double dtmp;
	d1 = distance[1];
	d2 = distance[2];
	if(d1>2000 || d2>2000)
		return 0;
	dtmp = (double)d1;
	dtmp -= (double)d2;
	dtmp = atan2(dtmp, (double)UltraSonic_Distance);
	if(angle == NULL)
	{
		dtmp *= 1000;
		if(dtmp<0)
			dtmp = -dtmp+1000000.00;
		printf("\r\n");
		PRINTF("Distance angle %ld.\r\n", (int32_t)dtmp);
		return 0;
	}
	*angle = (float)dtmp;
	return 1;
}

uint8_t DistanceGetAngleUsmart(void)
{
	return DistanceGetAngle(NULL)*180.00/PI;
}
